#include "ndt_match.h"
#include <ros/ros.h>
#include <iostream>
#include <iterator>
#include <cstdlib>
#include <csignal>
#include <sensor_msgs/PointCloud2.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ndt_sub");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");
    // 测试使用
    // 初始化一个Eigen::Matrix4f矩阵
    Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity();
    init_guess << 3, 2, 5, 0,
        4, 1, 6, -0.6,
        5, 8, 1, 20,
        0, 0, 0, 0;
    // 打印矩阵
    adjustTransformation(init_guess);
    std::cout<<"变换后" << init_guess << std::endl; 
}